/*=+--+=#=+--     Unmanned Aerial System Management Software      --+=#=+--+=#*\
|          Copyright (C) 2011 Regents of the University of Colorado.           |
|                             All Rights Reserved.                             |

     This program is free software: you can redistribute it and/or modify
     it under the terms of the GNU General Public License version 2 as
     published by the Free Software Foundation.

     This program is distributed in the hope that it will be useful,
     but WITHOUT ANY WARRANTY; without even the implied warranty of
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     GNU General Public License for more details.

     You should have received a copy of the GNU General Public License
     along with this program.  If not, see <http://www.gnu.org/licenses/>.

            Jack Elston                       Cory Dixon                        
|           elstonj@colorado.edu              dixonc@colorado.edu              |
|                                                                              |
\*=+--+=#=+--                 --+=#=+--+=#=+--                    --+=#=+--+=#*/

/***********************************************************************
 *
 * FILENAME:
 * gpsTest.cxx
 *
 * PURPOSE:
 *  Test gps interface on the computer
 *
 * LAST MODIFIED:
 * $Date: 2005/05/03 04:39:32 $
 * $Revision: 1.1.1.1 $
 * $Author: dixonc $
 ***********************************************************************/

#include "tornadoChaser.h"

#ifdef VXWORKS
# include "vxWorks.h"
# include "taskLib.h"
# include "ioLib.h"
# include "sysLib.h"
#else
# include <stdio.h>
# include <unistd.h>
# include <sys/ioctl.h>
#endif

#include <signal.h>
#include <errno.h>
#include <string>

#include "serial.h"
#include "command.h"
#include "timeLib.h"

//////////////////////////////////////////////////////////////////////
// Function declaration
//////////////////////////////////////////////////////////////////////

void run();
// connect to the gps board and send any
// initial setup bytes to perpare for running
bool connect();

// initialize the gps board and the serial port
// into the required state
int init();

// set the operational mode
void SetPolledMode(); 
void SetContinuousMode();

// checksum, which is 8 bit xor of all values
// ignore $ (first character)
// go to * before checksum      
uchar8 CalcCheckSum(const char *cmd, int numChar);
uchar8 gpsCheckSum(const string nmea);

// poll the gps for a message
bool pollGPS();

// this are mapped to GPS output commands
void SetPort();                             // 000
bool SelfTest();                            // 003
void RequestOutput(char *NMEAId);           // 004
void sigHandler(int signo, siginfo_t *info, void *extra);

#define GPS_232_BUF_SIZE   128
#define GPS_NMEA_BUF_SIZE  256

//////////////////////////////////////////////////////////////////////
// Global variables
//////////////////////////////////////////////////////////////////////
SerialPort serial;
int execRate = 1;

// buffer of received GPS
char NMEABuf[256];
char RS232Buf[GPS_232_BUF_SIZE];

// Poll Loop commands
// These commands are the ones polled by the gps, if
// GPS is in polled mode
//
#define GPS_NUM_CMD_STR 3
char *GPSPollStr[ GPS_NUM_CMD_STR ] = { "RMC", "GGA", "GSA" };
int GPSPollInd;

// this holds a local copy of the data of interest.
// This is copied to the shared memory area, which
// is eventually sent to the ground
GPSStruct gps;  

char portName[20] = GPS_PORT_STR;
int portBaud = GPS_PORT_BAUD;

//////////////////////////////////////////////////////////////////////
// Main
//////////////////////////////////////////////////////////////////////
#ifdef VXWORKS
int gpsTest()
#else
int main(int argc, char *argv[])
#endif
{

#ifndef VXWORKS
	if( argc == 3) {
		strcpy(portName, argv[1]);
		portBaud = atoi( argv[2] );
	}
#endif

	// init
	if ( init() != OK ) 
		perror("Could not initialize the GPS task");

	// run
	run();

	return 0;
}

//----------------------------------------------------------
// connect to the gps board
// send any initial setup bytes to perpare for
// running, init should have alredy been called
//----------------------------------------------------------
bool connect()
{
	if( serial.getStatus() != SerialPort::Connected )
		return false;

	// flush IO
	serial.flushIO();

	// do a self test() to see if it is there and on
	if( SelfTest() ){
		cout << "GPS Passed self test" << endl << flush;
		gps.status.connected = true;
	} else {
		cout << "GPSTask::connect - SelfTest failed" << endl << flush;
		gps.status.connected = false;
		return false;
	}

	// set polled or streamed mode
	gps.status.mode = STATUS_POLLED;

	//
	// set run mode
	//
	if(gps.status.mode == STATUS_POLLED) 
		SetPolledMode();
	else if(gps.status.mode == STATUS_STREAM)
		SetContinuousMode();

	return true;
}

//----------------------------------------------------------
// initialize the serial port and the gps board to the
// required state
//----------------------------------------------------------
int init( )
{
	cout << "GPS - initializing" << endl << flush;

	// initialize the serial port
	if( serial.init( portName, portBaud) != SerialPort::Connected) {
		cout << "GPSTask::init - could not init serial port" << endl << flush;
		return ERROR;
	} else
		cout << "GPS - opened serial port " << GPS_PORT_STR << " @ " << GPS_PORT_BAUD << endl;

	// since serial port puts into RAW mode, we need
	// to put it into line mode
	/*
	if ( ioctl(serial.getFd(), FIOSETOPTIONS, OPT_LINE)  == ERROR ) {
		cout << "GPSTask::init - could not set line option" << endl;
		return ERROR;
	}
	*/

	// normally we would turn on gps here
	gps.status.on = OK;

	// try to connect to gps 
	cout << "GPS - Trying to connect" << endl << flush;
	if( connect() )
		cout << "GPS - Connected" << endl << flush;
	else {
		cout << "GPS - did not connect" << endl << flush;
		return ERROR;
	}

	return OK;
}


//----------------------------------------------------------
// Run function
//----------------------------------------------------------
void run()
{
    cout << "\t" << ": runRate=" << execRate << " [Hz]"
        << endl << flush;

	// interval timer if in polled mode
	// since gps updates only 1 a second, we want
	// to run at the rate of the number of strings
	// we want, i.e. if two NMEA commands run a 2.0 Hz
	IntervalTimer it( (float)GPS_NUM_CMD_STR );

	// setup signals
	//  SIGALRM - timer signal for running
	//  SIGINT  - from user as control-C
	//  TSIGQUIT - from code to exit threads
	sigset_t waitSigs;
	siginfo_t extra;
	int sig;

	sigemptyset(&waitSigs);
	sigaddset(&waitSigs,SIGALRM);
	sigaddset(&waitSigs,TSIGQUIT);
	sigprocmask(SIG_BLOCK,&waitSigs,NULL);

	//----------------------------------------------------------
	// catch the signal TSIGQUIT
	//  this signal is merely to interrupt
	//  a thread so it will shutdown
	//----------------------------------------------------------
	struct sigaction sa;
	sigemptyset(&sa.sa_mask);
	sa.sa_flags = SA_SIGINFO;
	sa.sa_sigaction = sigHandler;

	if( sigaction(TSIGQUIT, &sa, NULL) < 0)
		perror("pThread::run sigaction");

	// start timer
	if(gps.status.mode == STATUS_POLLED)
		it.start();

	int comBytes = 0;
	SerialPort::SerialWait wval = SerialPort::WAIT_ERROR;

	while(execRate > 0) { 
		//
		// get the most up-to-date values, and reset others
		//
		wval = SerialPort::WAIT_ERROR;

		if( gps.status.connected ) {
			if( gps.status.mode == STATUS_STREAM) {
				// wait on select for 2 seconds
				wval = serial.wait(2000);

				if( wval == SerialPort::WAIT_TIMEOUT )
					cout << "serial.wait() timeout " << endl;
				else if( wval == SerialPort::WAIT_ERROR )
					cout << "serial.wait() error" << endl;
				else if( wval == SerialPort::WAIT_DATA ){

					// read some bytes from the port
					comBytes = serial.read(RS232Buf, GPS_232_BUF_SIZE);

					// process the received bytes
					RS232Buf[comBytes] = '\0';
					printf("GPS: %s\n", RS232Buf);

				}
			}

			// handle signals, which can occur above during
			// select or by being in polled mode the timer signal
			// or others
			if( gps.status.mode == STATUS_POLLED || wval == SerialPort::WAIT_INT ) {
				//
				// wait for run signal
				//
				if( (sig = sigwaitinfo(&waitSigs,&extra)) < 0) {
					if( errno != EAGAIN & errno != EINTR ) {
						cout << " sigwaitinfo()" << endl;
						execRate = 0;
					}
				} else if (sig == SIGINT) {
					// don't do anything, as this is control-c and
					// is caught by higher level tasks
					cout << " caught sigint" << endl;
				} else if (sig == SIGQUIT) {
					// should not be here
					cout << "should have been caugt by handler" << endl;
				} else if (sig == SIGALRM) {
					// poll the gps
					pollGPS();
				}
			}

		} else {
			// wait
#ifdef VXWORKS
			taskDelay(sysClkRateGet());
#else
			sleep(1);
#endif
		}

	} // end of while()
	
	it.stop();
}                             
							 
//----------------------------------------------------------
// Poll GPS
//----------------------------------------------------------
bool pollGPS()
{
	//
	// make first request to start polling loop
	//
	printf("GPSTask::pollGPS() - polling with: %s\n", GPSPollStr[GPSPollInd]);
	RequestOutput( GPSPollStr[GPSPollInd] );
	GPSPollInd = (GPSPollInd + 1 ) % GPS_NUM_CMD_STR;

	// wait for response
	int n=0;

	if( serial.wait(1000) == SerialPort::WAIT_DATA ) {
		if( (n = serial.read(RS232Buf, GPS_232_BUF_SIZE)) > 0 ) {

			RS232Buf[n]='\0';
			printf("GPS: %s\n", RS232Buf);

			return true;
		} else
			perror("pollGPS() - serial.read no bytes returned");
	} else 
		perror("pollGPS() - serial.read failed");
 		return false;
}

void SetPolledMode()
{
	printf("SetPolledMode() - configuring GPS\n");

	const int maxLength = 50;
	int pos=0;
	char cmdBuffer[maxLength + 1]= {'\0'};

	// sends RMC-1s, GGA-1s, GSA-1s
	// save in NVRAM also ( the '1' )
	pos = sprintf((char *)cmdBuffer,"$PMCAG,005,1,RMC,000,GGA,000,GSA,000"); 

	// add end of NMEA command values
	pos += sprintf(&(cmdBuffer[pos]),"*%2X",CalcCheckSum(cmdBuffer,pos) );

	cmdBuffer[pos++] = 13; // <CR>
	cmdBuffer[pos++] = 10; // <LF>
	cmdBuffer[pos] = '\0'; // NULL

	printf("GPSTask::SetPolledMode() - sending NMEA command: \n\t%s\n",cmdBuffer);

	if( serial.write(cmdBuffer, pos) != pos){
		printf( "GPSTask::SetPolledMode() - could not write to serial port\n");
	}

	gps.status.mode = STATUS_POLLED;

	printf("GPSTask::SetPolledMode() - set polled mode\n"); 
}

void SetContinuousMode()
{
	printf("SetContinuousMode() - configuring GPS\n");

	const int maxLength = 50;
	int pos=0;
	char cmdBuffer[maxLength + 1]= {'\0'};

	// sends RMC-1s, GGA-1s, GSA-1s
	// save in NVRAM also ( the '1' )
	pos = sprintf((char *)cmdBuffer,"$PMCAG,005,1,RMC,001,GGA,001,GSA,001"); 

	// add end of NMEA command values
	pos += sprintf(&(cmdBuffer[pos]),"*%2X",CalcCheckSum(cmdBuffer,pos) );

	cmdBuffer[pos++] = 13; // <CR>
	cmdBuffer[pos++] = 10; // <LF>
	cmdBuffer[pos] = '\0'; // NULL

	printf("GPS_SetContinuousMode() - sending NMEA command: \n\t%s\n",cmdBuffer);

	if( serial.write(cmdBuffer, pos) != pos){
		printf("GPS_SetContinuousMode - could not write to serial port\n");
	}

	gps.status.mode = STATUS_STREAM;
}


uchar8 CalcCheckSum(const char *cmd, int pos)
{
	unsigned char val=0;
	// pg 5-30 of Marconi Manual

	// calc checksum, which is 8 bit xor of all values
	// ignore $ (first character)
	// go to * before checksum      
	for(int i=1; i < pos; i++)
		val ^= cmd[i];

	return val;
}

#define GPS_GPS_BAUD_CHAR '6'

void SetPort()
{
	// Page 5-33 Marconi Manual
	// this is command $PMCAG,000
	// length is 17
	const int maxLength = 17;
	int pos=0;
	char cmdBuffer[maxLength + 1]= {'\0'};

	pos = sprintf((char *)cmdBuffer,"$PMCAG,000,%c",GPS_GPS_BAUD_CHAR); 

	// add end of NMEA command values
	pos += sprintf(&(cmdBuffer[pos]),"*%2X",CalcCheckSum(cmdBuffer,pos) );

	cmdBuffer[pos++] = 13; // <CR>
	cmdBuffer[pos++] = 10; // <LF>
	cmdBuffer[pos] = '\0'; // NULL

	printf("GPS_SetPort() - sending NMEA command: \n\t%s\n",cmdBuffer);

	if( serial.write(cmdBuffer, pos) != pos){
		printf("GPS_SetPort - could not write to serial port\n");
	}

}


void RequestOutput(char *nmeaId)
{
	// Page 5-36 Marconi Manual
	// this is command $PMCAG,004
	// length is 19
	int pos=0;
	char gpsOutputBuf[256];
	pos = sprintf(gpsOutputBuf,"$PMCAG,004,%s",nmeaId); 

	// add end of NMEA command values
	pos += sprintf(&(gpsOutputBuf[pos]),"*%2X",CalcCheckSum(gpsOutputBuf, pos) );

	gpsOutputBuf[pos++] = 13; // <CR>
	gpsOutputBuf[pos++] = 10; // <LF>
	gpsOutputBuf[pos] = '\0'; // NULL

	printf("RequestOutput() - sending NMEA command: \t%s => %d bytes\n", 
			gpsOutputBuf, pos);

	if( serial.write(gpsOutputBuf,pos) != pos){
		perror("RequestOutput - could not write to serial port\n");
	}
}

bool SelfTest()
{
	// Page 5-35 Marconi Manual
	// this is command $PMCAG,003
	// length is 15
	const int maxLength = 15;
	int pos=0;
	char cmdBuffer[maxLength + 1]= {'\0'};

	pos = sprintf(cmdBuffer,"$PMCAG,003"); 

	// add end of NMEA command values
	pos += sprintf(&(cmdBuffer[pos]),"*%2X",CalcCheckSum(cmdBuffer,pos) );

	cmdBuffer[pos++] = 13; // <CR>
	cmdBuffer[pos++] = 10; // <LF>
	cmdBuffer[pos++] = '\0'; // NULL

	printf("GPSTask::SelfTest() - sending NMEA command: \n\t%s\n", cmdBuffer);

	if( serial.write(cmdBuffer,pos) != pos){
		printf("GPSTask::SelfTest - could not write to serial port\n");
	}

	printf("GPSTask::SelfTest - wrote to serial port\n");

	// wait for response
	int n=0;
	int length = 255;
	char buf[length + 1];

	// the self test can take up to two seconds
	if( serial.wait(5000) == SerialPort::WAIT_DATA ) {
		if( (n = serial.read(buf, length)) > 0 ) {
			buf[n] = '\0';
			printf("Result of Self Test - %s",buf);

			return true;
		} else
			perror("SelfTest() - serial.read");
	} 

 	return false;
}

uchar8 gpsCheckSum(const string nmea)
{
	// calc checksum, which is 8 bit xor of all values
	// ignore $ (first character)
	// go to * before checksum      

	uchar8 csum = 0;
	string::size_type start = nmea.find_first_of("$");
	string::size_type stop   = nmea.find_first_of("*");

	if( start == string::npos || stop == string::npos) {
		cout << "Could not find start/stop of NMEA string" << endl;
		return 0;
	} else
		for( register unsigned int i = start + 1; i < stop; i++)
			csum ^= nmea[i];

	return csum;
}


void sigHandler(int signo, siginfo_t *info, void *extra)
{
        //void *me = info->si_value.sival_ptr;
}

